#ifndef ENGINECONTROL_H
#define ENGINECONTROL_H
#include <Arduino.h>
#include "InterruptHandler.h"
#include "TimerEvenetQueue.h"


class Encoder: public InterruptHandler
{
public:
    unsigned long getmeasuredTime(){return _measuredTime;}
    unsigned long getTimeFromLastMeasurement()
    {
      return micros()-_timestamp;
    }
    Encoder(short encoderPin, Callback* callback)
    :InterruptHandler(encoderPin),_timestamp(0),_measuredTime(0),_encoderPin(encoderPin), _callback(callback)
    {
    }
    virtual ~Encoder()
    {
    }
    void onInterrupt() //bedzie potezny blad raz na ~70 minut, ale nie ma potrzeby sie nim przejmowac
    {
        measurementUpdate();
    }
    void measurementUpdate()
    {
        if(digitalRead(_encoderPin)==HIGH)
        {
            unsigned long time = micros();
            _measuredTime = time - _timestamp;
            _timestamp = time;
        }
        _callback->callbackFunc();
    }

private:
    short _encoderPin;
    unsigned long _measuredTime;
    unsigned long _timestamp;
    Callback* _callback;
};




class EngineControl :public Callback, public TimerEventListener
{
public:
  EngineControl(short pinF, short pinB, int minTimeInMicros, short encoderPin)
  :_timeSetForCurrSpeed(0), _currPWMSpeed(0),
  _pinF(pinF),_pinB(pinB),_activePin(pinF),
    _minTimeInMicros(minTimeInMicros), _percent(0), _encoder(encoderPin, this)
  {
  }

  unsigned long int getTimeInMicros(){return _encoder.getmeasuredTime();}
  unsigned short getPWM(){return _currPWMSpeed;}
  unsigned short getPercent(){return _percent;}
  unsigned long getCurrentTime(){return _timeSetForCurrSpeed;}
  void callbackFunc()
  {
    _measurementReady = true;
  }
  void onTime()
  {
      if(_measurementReady)
        periodicSpeedUpdate();
      _measurementReady = false;
  }
  void periodicSpeedUpdate()
  {
    if(_percent != 0)
    {
        preventStalling();
        updateSpeed();
    }
    else stopEngine();

  }
  unsigned long setMotorSpeed(int percent)
  {
    _percent = percent;
    if(percent != 0)
    {
      calculatePulseTime();
      if(percent < 0)//jedziemy w tyl, pin "przedni" na stop i update predkosci
      {
          shiftBack();
      }
      else
      {
          shiftForward();
      }
    }
    periodicSpeedUpdate();
    return _timeSetForCurrSpeed;
  }
  void preventStalling()
  {

      if(_encoder.getTimeFromLastMeasurement() > _timeSetForCurrSpeed * 2 && _percent != 0)
      {
        //Serial.println("stall");
        if(_currPWMSpeed + 5 < 255)
        {
          _currPWMSpeed +=  5;
          analogWrite(_activePin, _currPWMSpeed);
        }
        updateSpeed();
      }
  }

private:

  inline unsigned long calculatePulseTime()
  {
      _timeSetForCurrSpeed = (100./(double)abs(_percent)) * _minTimeInMicros;
  }
  inline void shiftForward()
  {
      _activePin = _pinF;
      analogWrite(_pinB,0);
  }
  inline void shiftBack()
  {
      _activePin = _pinB;
      analogWrite(_pinF,0);
  }
  inline void accelerate()
  {
    _currPWMSpeed >=255 ? 255 : _currPWMSpeed++;
    analogWrite(_activePin, _currPWMSpeed);
  }
  inline void decelerate()
  {
    _currPWMSpeed == 0 ? 0 : _currPWMSpeed--;
    analogWrite(_activePin, _currPWMSpeed);
  }
  void stopEngine(){
        analogWrite(_pinB,0);
        analogWrite(_pinF,0);
        _currPWMSpeed = 0;
        _percent=0;
  }
  inline bool tooSlow(){return _encoder.getmeasuredTime() > _timeSetForCurrSpeed;}
  inline bool tooFast(){return _encoder.getmeasuredTime() < _timeSetForCurrSpeed;}
  void updateSpeed()
  {
    if(tooSlow())
        accelerate();
    else if(tooFast())
        decelerate();
  }

  unsigned long _timeSetForCurrSpeed;
  unsigned short _currPWMSpeed;
  short _percent;
  unsigned short _pinF;
  unsigned short _pinB;
  unsigned short _activePin;
  unsigned long _minTimeInMicros;
  Encoder _encoder;
  bool _measurementReady;
};

#endif // ENGINECONTROL_H
